#ifndef TASK1_PATH_H
#define TASK1_PATH_H

#include "bits/stdc++.h"
#include "eigen3/Eigen/Eigen"
#include "msg.h"
#include "unistd.h"

#define Pi         3.1415926
#define max_length 2.4

#define HEAD   0
#define BACK   1
#define SIDE   2
#define CEFANG 8
#define DAOCHE 9
#define END    10

using namespace std;
/**
 * \name dot
 * \public
 * 横坐标：x,纵坐标：y\n
 * 极    径：L, 极  角：angle\n
 * fill_dot：填充成员变量\n
 * \private
 * get_distance用于计算极坐标的L
 * \attention
 * 重载==运算符
 */
class dot
{
public:
    double x{}, y{};
    double l{};
    double angle{};

    dot() = default;

    dot(double l1, double angle1)
    {
        l     = l1;
        angle = angle1;
        x     = l * sin(angle);
        y     = l * cos(angle);
    }

    dot(double x1, double y1, int id1)
    {
        x = x1;
        y = y1;
        l = get_distance();
        if ( abs(x) < 0.000001 ) {
            angle = 0;
        }
        else {
            angle = x * y > 0 ? Pi / 2 - atan(y / x) : -Pi / 2 - atan(y / x);
        }
    }

    void fill_dot()
    {
        l = get_distance();
        if ( abs(x) < 0.000001 ) {
            angle = Pi / 2;
        }
        else {
            angle = x * y > 0 ? Pi / 2 - atan(y / x) : -Pi / 2 - atan(y / x);
        }
    }

    friend ostream& operator<<(ostream& output, const dot& dot1)
    {
        output << "x : " << dot1.x << "  y : " << dot1.y << "  L : " << dot1.l
               << "  angle : " << dot1.angle << endl;
        return output;
    }

private:
    double get_distance() const { return sqrt(x * x + y * y); }

    bool operator==(const dot& a) const
    {
        return (x == a.x && y == a.y) || (l == a.l && angle == a.angle);
    }
};

/**
 * \name line
 * \public
 * 包含点的信息：vector<dot> dots\n
 * 中点信息：dot_mid\n
 * 直线斜率： k 与  b \n
 * fill_line() : 计算k，b与中点并填充
 */
class line
{
public:
    vector<dot> dots;
    dot         dot_mid;
    double      k{}, b{};

    line() = default;
    void fill_line();
    bool operator<(const line a) const
    {
        return this->dots.size() > a.dots.size();
    }
};

/**
 * \name path
 * \public
 * flag：判断停车是否完成\n
 * car_cur：存储当前车辆位置\n
 * vpoints：存储生成的预测轨迹\n
 * \note 使用示例：\n
 * int main(){\n
 *     auto *path = new path(msg);\n
 *     while(1){\n
 *         path.count_path(msg);\n
 *         delay_ms(10);\n
 *         double t = trace_position();\n
 *         USART_SEND(t);\n
 *     }
 * }
 */
class path
{
public:
    int            flag{}, type{};
    dot            car_cur;
    vector<dot>    vpoints;
    vector<double> dvp;

    path() { flag = 0; }
    explicit path(int type1)
    {
        flag = 0;
        type = type1;
    }

    // 误差控制计算
    int count_path(my_msg::LaserScans* msg);
    int generate(my_msg::LaserScans* msg);

private:
    void generate_head();
    void generate_back();
    void generate_side();

    void   generate_vpoints(int InsertNum);
    double trace_two_sign();
    double trace_one_sign();
    void   generate_dvp();

    int        get_position(my_msg::LaserScans* msg);
    inline int count_one_sign(vector<line>& lines);
    inline int count_two_sign(vector<line>& lines);

    inline int count_side_sign(vector<line>& lines);

    // 求解矩阵辅助函数
    Eigen::Matrix<double, 4, 1> mat_count(dot& PL, dot& PR);
    // 三次B样条曲线辅助函数
    static double F03(double t)
    {
        return 1.0 / 6 * (-t * t * t + 3 * t * t - 3 * t + 1);
    }
    static double F13(double t)
    {
        return 1.0 / 6 * (3 * t * t * t - 6 * t * t + 4);
    }
    static double F23(double t)
    {
        return 1.0 / 6 * (-3 * t * t * t + 3 * t * t + 3 * t + 1);
    }
    static double F33(double t) { return 1.0 / 6 * t * t * t; }
};

#endif   // TASK1_PATH_H
